#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     HTDIR,          sensorI2CMuxController)
#pragma config(Sensor, S2,     TOUCH,          sensorNone)
#pragma config(Sensor, S3,     HTDIR,          sensorNone)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorH,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


int cleanup(int x) {
  const int threshold = 10;
  if (x > threshold)
    return (x - threshold) * (x - threshold) /  128;
  if (x < -threshold)
    return (x + threshold) * (x + threshold) / -128;
  return 0;
}



task main()
{


  //waitForStart();


  float servostate = servo[servo1];
  float clawDivisor = 300;

  while (true)
  {
    getJoystickSettings(joystick);
		nxtDisplayString(1, "x:%d y:%d", joystick.joy1_x1, joystick.joy1_y1);
		nxtDisplayString(2, "x:%d y:%d", joystick.joy1_x2, joystick.joy1_y2);

    int x  = cleanup(joystick.joy1_y1);
    int y  = cleanup(joystick.joy1_x1);
		nxtDisplayString(5, "s:%d", servo[servo1]);

    motor[motorH] = ;
    motor[motorI] = ;
    motor[motorH] = ;
    motor[motorI] = ;

    int s = cleanup (joystick.joy2_y2);

    bFloatDuringInactiveMotorPWM = false;
	}
}
